#ifndef INFO_PROCESS_HPP
#define INFO_PROCESS_HPP
#include <chrono>
#include <memory>
#include <iostream>
#include <vector>
#include <math.h>
#define R_F_MOT 0
#define R_B_MOT 1
#define L_B_MOT 2
#define L_F_MOT 3
const float PI=3.1415927;
const float speed_change=50.0;
inline double  calcDistance(double x, double y)
{
    return sqrt(x * x + y * y);
} 

inline int sgn(double x) 
{
    if(x >= 0) return 1;
    if(x < 0) return -1;

    return 0;
}
class Motor_Info_Press
{
    public:
        Motor_Info_Press();
        ~Motor_Info_Press(){}
        //将轮速 rmp 转换成 m/s
      float convert_rmp_to_ms(int sp); 
    //将轮速m/s 转换成  rmp  
      int convert_ms_to_rmp(float fsp) ; 
      void Driver_cal_real(float lx,float ly,float wmga);
      float convert_yaogan_wmga( int16_t w);
      float convert_yaogan_line(int16_t v);
      void run_set_angle(void);
      void convert_line_real(void);
      //车体参数     
      double WHEEL_RADIUS = 0.100; //轮半径
      double WHEEL_BASE =1.60 ;          // 车长（轮距） 0.860;
      double TRACK =1.60;               // 车宽（轮距）  0.690;

      double RY = WHEEL_BASE / 2.f;
      double RX = TRACK / 2.f;
      float wmga_max = 0.3;
      float line_x = 1.0;  //1m/s
      double angle_c[4];                                    //计算结果-角度
      double angle_b[4];                                    //最优结果-角度

      double angle_n[4];                                    //当前结果-角度

      double rpm_c[4];                                      //计算结果-速度
      double rpm_b[4];      
      /////////////////////////////////////////
    int lun_speed_set[4];
    int lun_speed_cur[4];
    int zhuan_speed_set[4];
    int zhuan_speed_cur[4];
    int angle_cur[4];
    int angle_set[4];
    int lun_enable=1;
};

#endif